#ifndef MAIN_H
#define MAIN_H

#include <board.h>
#include <pio/pio.h>
#include <aic/aic.h>
#include <tc/tc.h>
#include <utility/trace.h>
#include <utility/led.h>
#include <efc/efc.h>
#include <pwmc/pwmc.h>
//#include <eefc/eefc.h>
#include <memories/flash/flashd.h>

#include "ahrs.h"

/*#ifndef unsigned short_type
  #define unsigned short_type
  typedef unsigned short unsigned short;
#endif*/

//------------------------------------------------------------------------------
//         VBus monitoring (optional)
//------------------------------------------------------------------------------
#if defined(PIN_USB_VBUS)

#define VBUS_CONFIGURE()  VBus_Configure()

#else
    #define VBUS_CONFIGURE()    USBD_Connect()
#endif

//------------------------------------------------------------------------------
//      Definitions
//------------------------------------------------------------------------------

// State machine
#define STATE_ACCEL_X     2
#define STATE_ACCEL_Y     3
#define STATE_ACCEL_Z     4
#define STATE_GYRO_X      5
#define STATE_GYRO_Y      6
#define STATE_GYRO_Z      7
#define STATE_MAG_X       8
#define STATE_MAG_Y       9
#define STATE_MAG_Z       10
#define STATE_CALCULATION 1

#define STATE_COUNT       7

// Work state
#define STATE_IDLE        0
#define STATE_RESUME      1

// ADC channels
#define ACCEL_X     0
#define ACCEL_Y     1
#define ACCEL_Z     2
#define GYRO_X      3
#define GYRO_Y      4
#define GYRO_Z      5
#define MAG_X       6
#define MAG_Y       7
#define MAG_Z       14
#define AIN10       15

unsigned int divtc1;

// Accelerometer Scale
//extern float IMU_ADC_ACCELX_SCALE;
//extern float IMU_ADC_ACCELY_SCALE;
//extern float IMU_ADC_ACCELZ_SCALE;

// Compass calibration variables. Call compass_init() after change
/*extern float IMU_ADC_MAGX_SCALE;
extern float IMU_ADC_MAGY_SCALE;
extern float IMU_ADC_MAGZ_SCALE;
extern unsigned short IMU_ADC_MAGX_ZERO;
extern unsigned short IMU_ADC_MAGY_ZERO;
extern unsigned short IMU_ADC_MAGZ_ZERO;*/

// SPI pins.
const Pin pinsSPI[] = {BOARD_SPI_PINS};

// ADC Data Ready pin
const Pin pinADCDataReady = PIN_ADC_READY;

// Reset for Gyro and Compass
const Pin pinMagReset = PIN_MAG_RESET;
const Pin pinGyroReset = PIN_GYRO_RESET;

// Accelerometer Sensitivity pins
const Pin pinAccelGS1 = PIN_ACCEL_GS1;
const Pin pinAccelGS2 = PIN_ACCEL_GS2;

const Pin pinMotor1 = PIN_PWM_MOTOR1;
const Pin pinMotor2 = PIN_PWM_MOTOR2;
const Pin pinMotor3 = PIN_PWM_MOTOR3;
const Pin pinMotor4 = PIN_PWM_MOTOR4;

float	X[7];
float	quat[4];
float	q0;
float	q1;
float	q2;
float	q3;
float	bias[3];

float   accel[3];
float   pqr[3];
float   mag[3]; // calibrated data from compass

float P[3], I[3], D[3];
float Isum[3], Imin[3], Imax[3];
float last_error[3];

//unsigned short gyro[3]; // raw data from gyro
//unsigned short accel_raw[3]; // raw data from accel

// Performs operations on last page (to avoid overriding existing program).
unsigned int lastPageAddress = AT91C_IFLASH + AT91C_IFLASH_SIZE - AT91C_IFLASH_PAGE_SIZE;
unsigned int* pLastPageData;

// Simple delay
inline void Delay (unsigned long a) { while (--a!=0); }

//------------------------------------------------------------------------------
/// Interrupt handler for TC0 interrupt. Increment program state
//------------------------------------------------------------------------------
void ISR_Tc0(void);

//------------------------------------------------------------------------------
/// Configure Timer Counter 0 to generate an interrupt.
//------------------------------------------------------------------------------
inline void ConfigureTc(void)
{
    unsigned int div;
    unsigned int tcclks;

    // Enable peripheral clock
    AT91C_BASE_PMC->PMC_PCER |= 1 << AT91C_ID_TC0;

    // Configure TC for a <<IMU_FREQUENCY>>Hz frequency and trigger on RC compare
    TC_FindMckDivisor(IMU_FREQUENCY * (STATE_COUNT - 1), BOARD_MCK, &div, &tcclks);
    TC_Configure(AT91C_BASE_TC0, tcclks | AT91C_TC_CPCTRG);
    AT91C_BASE_TC0->TC_RC = (BOARD_MCK / div) / (IMU_FREQUENCY * (STATE_COUNT - 1)); // timerFreq / desiredFreq

    // Configure and enable interrupt on RC compare
    AIC_ConfigureIT(AT91C_ID_TC0, AT91C_AIC_PRIOR_LOWEST, ISR_Tc0);
    AT91C_BASE_TC0->TC_IER = AT91C_TC_CPCS;
    AIC_EnableIT(AT91C_ID_TC0);
    
    // Enable peripheral clock
    AT91C_BASE_PMC->PMC_PCER |= 1 << AT91C_ID_TC1;

    // Configure TC for a 1000kHz frequency
    TC_FindMckDivisor(1000000, BOARD_MCK, &divtc1, &tcclks);
    TC_Configure(AT91C_BASE_TC1, tcclks | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_WAVE | AT91C_TC_ACPA_CLEAR | AT91C_TC_ACPC_SET);
    AT91C_BASE_TC1->TC_RC = (BOARD_MCK / divtc1) / (50); // timerFreq / desiredFreq
    AT91C_BASE_TC1->TC_RA = (BOARD_MCK / divtc1) / (1000);

    // Configure and enable interrupt on RC compare
    //AIC_ConfigureIT(AT91C_ID_TC1, AT91C_AIC_PRIOR_LOWEST, ISR_Tc1);
    //AT91C_BASE_TC1->TC_IER = AT91C_TC_CPCS;
    //AIC_EnableIT(AT91C_ID_TC1);
}

#endif
